#版本号 1.1.4

#导入模块
from typing import List, Tuple
from maix import camera, err,image,display,app,uart, touchscreen,gpio,pinmap
from maix._maix.image import COLOR_BLACK, COLOR_BLUE, COLOR_GREEN, COLOR_RED, COLOR_YELLOW
import math , struct
import numpy as np
import cv2,math

#类
class Laser:
    def __init__(self,thresholds:List[List[int]],err_threshold = 5,err = 5):
        self.now_position = ()         # 激光现在的位置
        self.last_position = ()        # 激光上一次的位置
        self.max_deviation = err       # 激光误识别误差像素
        self.target_position = ()      # 激光目标的位置
        self.index = 0                 # 激光目标位置的索引
        self.points = ()               # 激光目标位置的集合
        self.thresholds = thresholds   # 激光的阈值
        self.err_threshold = err_threshold # 激光追踪的误差值
        self.done = False              # 激光追踪完成标志位

    def set_points(self,points:Tuple):
        self.points = points

    def traceing_point(self,img:image.Image,target_point:Tuple,fact_position):
        if target_point:
            ex = 0
            ey = 0
            if not fact_position :
                return (5000,5000)
            img.draw_keypoints([int(target_point[0]),int(target_point[1])],color=COLOR_YELLOW)
            # 计算当前误差
            ex = target_point[0] - fact_position[0]
            ey = target_point[1] - fact_position[1]
            ex = int(-ex)
            ey = int(-ey)
            return ex , ey
        else :
            return 0,0

    def reset_task(self):
        self.done = False
        self.index = 0

    def traceing_box(self,img:image.Image,fact_position):
        if self.done or self.index >= len(self.points):   # 判断是否完成所有点的追踪
            self.done = True
            if self.points :
                return self.traceing_point(img,self.points[0],fact_position)

        ex = 0 
        ey = 0
        # 获取当前目标点
        if self.index < len(self.points):
            target_point = self.points[self.index]
            img.draw_keypoints([int(target_point[0]),int(target_point[1])],color=COLOR_YELLOW)
            print(int(target_point[0]),int(target_point[1]))

            ex,ey = self.traceing_point(img,target_point,fact_position)
            #对误差进行判断
            if -self.err_threshold < ex < self.err_threshold and -self.err_threshold < ey < self.err_threshold:
                self.index = self.index + 1
            
            self.done = ( self.index >= len(self.points) )

        return ex , ey
    
    def get_laser_position(self,img:image.Image,target_threshold:list[list[int]]):
        blobs = img.find_blobs(thresholds=target_threshold,area_threshold=2,pixels_threshold=2)
        if blobs:
            blob = max(blobs , key = lambda blob:blob.area()) #找最大的色块
            img.draw_rect(blob[0],blob[1],blob[2],blob[3],color=COLOR_GREEN)
            #获得激光现在的位置
            self.now_position = (
                blob.cx(),
                blob.cy()
            )
            img.draw_cross(self.now_position[0],self.now_position[1],color=COLOR_GREEN,size=5)
            
            print(self.now_position)
            return self.now_position

    '''
    def get_laser_position(self, img: image.Image, target_threshold: list[list[int]], max_deviation: int = 6):
    # 检测当前帧激光点
        blobs = img.find_blobs(thresholds=target_threshold, area_threshold=3, pixels_threshold=3, merge=True)
        current_position = None
        if blobs:
            blob = max(blobs, key=lambda b: b.area())
            img.draw_keypoints([blob.cx(),blob.cy()],COLOR_GREEN,size= 8)
            current_position = (blob.cx(), blob.cy())
            print(current_position)
            
    
        # 情况1：无激光点 → 返回None（明确丢失）
        if not current_position :
            print("情况1")
            return None
    
        # 情况2：首次检测 → 初始化位置
        if not self.last_position :
            print("情况2")
            self.last_position = current_position
            self.now_position = current_position
            return current_position

        if self.last_position:
        # 情况3：位移校验
            print("情况3")
            dx = current_position[0] - self.last_position[0]
            dy = current_position[1] - self.last_position[1]
            distance_sq = dx**2 + dy**2
    
            if distance_sq <= max_deviation**2:  # 位移在容差内
                self.now_position = current_position
                self.last_position = current_position  # 更新历史位置
                return current_position
            else:  # 位移超限 → 视为干扰，返回历史位置
                print(f"位移超限！偏差{math.sqrt(distance_sq):.1f}像素 > {max_deviation}")
                return self.last_position  # 关键修改：返回历史位置而非None
    '''
class Box:
    def __init__(self, target_threshold: List[List[int]]):
        self.points = ()  # 使用元组存储点坐标
        self.corners = () # 使用元组存储四个角点坐标
        self.thresholds = target_threshold

    def form_rectangle_opencv(self,points: List[List[int]]) -> Tuple[Tuple[int, int], ...]:
        """
        输入：4个点的列表，格式 [[x1,y1], [x2,y2], [x3,y3], [x4,y4]]
        输出：有序矩形角点的嵌套元组 ((左上), (右上), (右下), (左下))，坐标为整型
        """
        points_np = np.array(points, dtype=np.float32)
    
        # 1. 计算最小外接旋转矩形
        rect = cv2.minAreaRect(points_np)
        box = cv2.boxPoints(rect)  # 获取四个浮点型角点
    
        # 2. 按y坐标排序（图像坐标系y轴向下）
        sorted_box = box[np.argsort(box[:, 1])]
    
        # 3. 处理顶部和底部点
        top_points = sorted_box[:2][np.argsort(sorted_box[:2, 0])]        # 左上→右上
        bottom_points = sorted_box[2:][np.argsort(sorted_box[2:, 0])[::-1]]  # 右下→左下
    
        # 4. 组合并转换为整型嵌套元组
        ordered_points = np.vstack([top_points[0], top_points[1], bottom_points[0], bottom_points[1]])
        int_points = np.round(ordered_points).astype(int)  # 浮点转整型（四舍五入）
    
        # 5. 明确创建二元组 (x, y) 结构
        return tuple(
                        (int(point[0]), int(point[1])) for point in int_points
                    )

    def shrink_rectangle_corners(
                                    self,
                                    corners: Tuple[Tuple[int, int], ...], 
                                    shrink_pixels: int = 2
                                ) -> Tuple[Tuple[int, int], ...]:
        """
        对矩形角点进行内缩处理（向内收缩固定像素）
    
        参数:
            corners: 矩形角点，格式为 ((左上), (右上), (右下), (左下))
            shrink_pixels: 内缩像素数（默认2像素）
    
        返回:
            内缩后的角点，格式与输入一致
        """
        # 1. 将元组转换为NumPy数组 [7,8](@ref)
        pts_np = np.array(corners, dtype=np.float32)
    
        # 2. 计算矩形中心点（所有点的均值）
        center = np.mean(pts_np, axis=0)
    
        # 3. 计算每个角点到中心的单位向量
        vectors = center - pts_np
        norms = np.linalg.norm(vectors, axis=1, keepdims=True)
        # 避免除零错误（若两点重合则跳过移动）
        valid_mask = (norms > 1e-5).reshape(-1)
        unit_vectors = np.zeros_like(vectors)
        unit_vectors[valid_mask] = vectors[valid_mask] / norms[valid_mask]
    
        # 4. 沿向量方向内缩角点
        shrunk_pts = pts_np + unit_vectors * shrink_pixels
    
        # 5. 重新排序为 (左上, 右上, 右下, 左下) [1,8](@ref)
        # 按y坐标排序（图像坐标系y轴向下）
        sorted_idx = np.argsort(shrunk_pts[:, 1])
        top = shrunk_pts[sorted_idx[:2]][np.argsort(shrunk_pts[sorted_idx[:2], 0])]
        bottom = shrunk_pts[sorted_idx[2:]][np.argsort(shrunk_pts[sorted_idx[2:], 0])[::-1]]
        ordered_pts = np.vstack([top[0], top[1], bottom[0], bottom[1]])
    
        # 6. 转为整型并返回嵌套元组
        return tuple(
                        (int(round(pt[0])), int(round(pt[1])))  # 明确创建 (x, y) 结构
                        for pt in ordered_pts
                    )



    def get_rect_corners(self, img: image.Image,n:int):

        target_points = ()
        
        black_blobs = img.find_blobs(thresholds=self.thresholds,pixels_threshold= 3000)
        #img.binary(self.thresholds)
        if black_blobs:
            out_rect = max(black_blobs , key = lambda blob :blob.pixels())  #屏幕外框
            out_corners = out_rect.corners()
            for i in range(4):
                img.draw_line(out_corners[i][0], out_corners[i][1], 
                                  out_corners[(i+1)%4][0], out_corners[(i+1)%4][1], COLOR_RED)

            in_corners = self.form_rectangle_opencv(out_corners)
            in_corners = self.shrink_rectangle_corners(in_corners,5)
            for i in range(4):
                img.draw_line(in_corners[i][0], in_corners[i][1], 
                              in_corners[(i+1)%4][0], in_corners[(i+1)%4][1], COLOR_BLUE)
            #获得角点坐标
            target_points = (
                    (in_corners[0][0],in_corners[0][1]),
                    (in_corners[1][0],in_corners[1][1]),
                    (in_corners[2][0],in_corners[2][1]),
                    (in_corners[3][0],in_corners[3][1])
                )

            #绘画四个角点
            img.draw_keypoints([in_corners[0][0],in_corners[0][1]],COLOR_BLUE)
            img.draw_keypoints([in_corners[1][0],in_corners[1][1]],COLOR_GREEN)
            img.draw_keypoints([in_corners[2][0],in_corners[2][1]],COLOR_RED)
            img.draw_keypoints([in_corners[3][0],in_corners[3][1]],COLOR_YELLOW)

                #for i in range(4):
                    #img.draw_line(target_points[i][0], target_points[i][1], 
                                  #target_points[(i+1)%4][0], target_points[(i+1)%4][1], COLOR_BLUE)
            if target_points :
                self.corners = target_points
                
        return target_points
        
    def draw_self_points(self):
        if  self.corners:
            #绘画四个角点
            img.draw_keypoints([self.corners[0][0],self.corners[0][1]],COLOR_BLUE)
            img.draw_keypoints([self.corners[1][0],self.corners[1][1]],COLOR_GREEN)
            img.draw_keypoints([self.corners[2][0],self.corners[2][1]],COLOR_RED)
            img.draw_keypoints([self.corners[3][0],self.corners[3][1]],COLOR_YELLOW)    
            for i in range(4):
                    img.draw_line(self.corners[i][0], self.corners[i][1], 
                                  self.corners[(i+1)%4][0], self.corners[(i+1)%4][1], COLOR_BLUE)


    def divide_rectangle(self, 
                         A: Tuple[int, int], 
                         B: Tuple[int, int], 
                         C: Tuple[int, int], 
                         D: Tuple[int, int], 
                         n: int):
        """
        计算矩形四条边的等分点（连接点不重复）
        :param n: 每条边的等分段数
        :return: 元组嵌套元组结构 (点1, 点2, ...)
        """
        # 计算各边的等分点（返回元组）
        AB = self.divide_segment(A, B, n, include_end=False)  # AB 不包含B点
        BC = self.divide_segment(B, C, n, include_end=False)  # BC 不包含C点
        CD = self.divide_segment(C, D, n, include_end=False)  # CD 不包含D点
        DA = self.divide_segment(D, A, n, include_end=True)   # DA 包含A点（闭合矩形）
        
        # 合并所有点并去重（返回元组）
        all_points = AB + BC + CD + DA
        self.points = self._remove_duplicates(all_points)
        return self.points + (A,)

    def divide_segment(self, 
                      start: Tuple[int, int], 
                      end: Tuple[int, int], 
                      n: int, 
                      include_end: bool = True) -> Tuple[Tuple[int, int], ...]:
        """
        计算线段等分点（返回元组）
        :param include_end: 是否包含终点
        :return: 元组嵌套元组 ((x1,y1), (x2,y2), ...)
        """
        start_arr = np.array(start)
        end_arr = np.array(end)
        num_points = n + 1 if include_end else n
        t = np.linspace(0, 1, num_points)
        points = start_arr + t[:, np.newaxis] * (end_arr - start_arr)
        
        # 将每个点转为整数坐标的元组，再包装成外层元组
        return tuple(tuple(p.astype(np.int32)) for p in points)

    def _remove_duplicates(self, 
                          points: Tuple[Tuple[int, int], ...]) -> Tuple[Tuple[int, int], ...]:
        """去重并保留顺序（返回元组）"""
        seen = {}
        unique_points = []
        for p in points:
            if p not in seen:
                seen[p] = True
                unique_points.append(p)
        return tuple(unique_points)  # 最终返回元组嵌套元组


def uart_send(serial:uart.UART,head,data:list):
    send_data = struct.pack("<Bhh",head,data[0],data[1])
    serial.write(send_data)

red_laser = Laser(thresholds=[[50, 92, 3, 34, -7, 23]],err_threshold = 10)  # 红色激光实例
#red_laser = Laser(thresholds=[[15, 29, 18, 52, 18, 52]],err_threshold = 20)  # 红色激光实例
cam = camera.Camera()
disp = display.Display()
black_box = Box([[0,36,-10,11,-10,10]])               # 黑色电工胶方框实例
test_box = Box([[0,39,0,21,-52,-2]])                 # 测试所用的矩形实例
device1 = "/dev/ttyS0"          # 示例的串口名称                        此串口设备名称为串口0
serial = uart.UART(device1,115200)  #创建串口对象，八位数据，无检验，一位停止
ts = touchscreen.TouchScreen()      #创建触摸屏对象
'''创建按键对象'''
pinmap.set_pin_function("A28","GPIOA28")
pinmap.set_pin_function("A29","GPIOA29")
pinmap.set_pin_function("A19","GPIOA19")
pinmap.set_pin_function("A18","GPIOA18")
key1 = gpio.GPIO("GPIOA19",gpio.Mode.IN)
key2 = gpio.GPIO("GPIOA18",gpio.Mode.IN)
key3 = gpio.GPIO("GPIOA29",gpio.Mode.IN)
key4 = gpio.GPIO("GPIOA28",gpio.Mode.IN)

'''画面处理部分'''
cam.luma(23)  # 调暗摄像头的亮度
cam.constrast(30)		# 设置对比度，范围[0, 100]
cam.saturation(30)		# 设置饱和度，范围[0, 100]
cam.exp_mode(0)

sport_state = 0 #激光运动状态
run_count   = 0 #运行统计
run_task = 2   #任务 (1:复位  2:围绕铅笔  3:围绕黑色电工胶  4:测试追点  5:测试追一个框)
is_set_points = True #是否重新设置追踪点

reset_positon = ()
red_position = ()
target_points = ()

cam.skip_frames(150)           # 跳过开头的30帧
while not app.need_exit():
    img = cam.read()
    img = img.crop(180,70,260,230)  # 将照片放大
    img.draw_string(0,0,"the versions is 1.1.4")  # 对照版本号
    ex = 0 # 发给单片机x轴的误差
    ey = 0 # 发给单片机y轴的误差

    red_position = red_laser.get_laser_position(img,red_laser.thresholds)
    
    if red_position:
        print("找到激光的位置")
    else :
        print("1111111111111111111111111111111111111111111111111111111111111111111111111")
        
    if is_set_points:
        is_set_points = False
        target_points = black_box.get_rect_corners(img,40) # 找黑色电工胶形成的黑框，并等分
        if target_points:
            target_points = black_box.divide_rectangle(target_points[0],target_points[1],target_points[2],target_points[3],40)
            if target_points:
                red_laser.set_points(target_points)
    else :
        #black_box.draw_self_points()
        #black_box.get_rect_corners(img,40) # 找黑色电工胶形成的黑框，并等分
        print(is_set_points)
    
    
    #points1 = test_box.divide_rectangle((100,100),(200,100),(200,200),(100,200),40)
    if black_box.corners and len(black_box.corners) >= 4:  # 确保有4个角点
        reset_positon = (
            int((black_box.corners[0][0] + black_box.corners[2][0]) / 2),
            int((black_box.corners[0][1] + black_box.corners[2][1]) / 2)
        )
        img.draw_keypoints([reset_positon[0],reset_positon[1]],color=COLOR_YELLOW)
    else:
        print("角点没有四个")
    img.save("/root/test/image.jpg")
    if not key1.value():
        print("按键一被按下")
        run_task = 1

    elif not key2.value():
        print("按键二被按下")
        run_task = 2
        
    elif not key3.value():
        print("按键三被按下")
        run_task = 3
        #red_laser.reset_task()
        
    elif not key4.value():
        print("按键四被按下")
        run_task = 4



    if run_task == 1:  
        ex,ey = red_laser.traceing_point(img,reset_positon,red_position)
        
    if run_task == 3 and  not target_points is None:
        #img = img.crop(180,70,320,240)  # 将照片放大
        ex,ey = red_laser.traceing_box(img,red_position)
        #print(target_points)
        
    if run_task == 4:
        '''
        x,y,pressed = ts.read()  #获取触摸数据
        n_x,n_y = image.resize_map_pos_reverse(img.width(),img.height(),disp.width(),disp.height(), image.Fit.FIT_CONTAIN,x,y)
        ex,ey = red_laser.traceing_point(img,(n_x,n_y),red_position)
        print(n_x,n_y)
        '''
        red_laser.reset_task() 
        is_set_points = True
            

    uart_send(serial=serial,data=[ex,ey],head=0xaa)

    print(f"y轴的误差是{ey}")
    print(f"x轴的误差是{ex}")
    disp.show(img=img)
